/*
MIT License

Copyright (c) 2025 sidianneng(bbear_mail@163.com)

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

#include "stepmotor_dir.h"
#include "freertos/FreeRTOS.h"
#include "freertos/queue.h"
#include "freertos/task.h"
#include "driver/gpio.h"
#include <stdio.h>

#define ESP_INTR_FLAG_DEFAULT 0

#define GPIO_INPUT_IO_0     1 
#define GPIO_INPUT_IO_1     2 
#define GPIO_INPUT_IO_2     3 
#define GPIO_INPUT_IO_3     4
#define GPIO_INPUT_IO_4     5
#define GPIO_INPUT_IO_5     6
#define GPIO_INPUT_IO_6     7
#define GPIO_INPUT_IO_7     8
#define GPIO_INPUT_PIN_SEL  ((1ULL<<GPIO_INPUT_IO_0) | (1ULL<<GPIO_INPUT_IO_1) | (1ULL<<GPIO_INPUT_IO_2) | (1ULL<<GPIO_INPUT_IO_3) \
                           | (1ULL<<GPIO_INPUT_IO_4) | (1ULL<<GPIO_INPUT_IO_5) | (1ULL<<GPIO_INPUT_IO_6) | (1ULL<<GPIO_INPUT_IO_7))

static QueueHandle_t gpio_evt_queue = NULL;
extern QueueHandle_t motor_ctl_queue;

//For xiaomi MJSXJ01CM
//static uint32_t m_clock[] = {
//                            0x03,//0011
//                            0x05,//0101
//                            0x0c,//1100
//                            0x0a,//1010
//                        };
//
//static uint32_t m_unclock[] = {
//                            0x0a,//1010
//                            0x0c,//1100
//                            0x05,//0101
//                            0x03,//0011
//                        };

//For xiaomi MJSXJ09CM
static uint32_t m_clock[] = {
                            0x01,
                            0x02,
                            0x04,
                            0x08,
                        };

static uint32_t m_unclock[] = {
                            0x08,
                            0x04,
                            0x02,
                            0x01,
                        };

static Step_Dir step_motor_direction(uint32_t *step_data, uint8_t len)
{
    Step_Dir stepdir = S_UNKNOWN;
    uint8_t clkindex = 0, unclkindex = 0;
    uint8_t clock_cnt = 0, unclock_cnt = 0;

    if(!step_data || !len) return S_UNKNOWN;

    for(uint8_t i = 0;i < len; i++) {
        if(step_data[0] == m_clock[i]) {
            clkindex = i;
            break;
        }
    }
    for(uint8_t i = 0;i < len; i++) {
        if(step_data[i] == m_clock[(clkindex + i) % len])
            clock_cnt++;
    }

    for(uint8_t i = 0;i < len; i++) {
        if(step_data[0] == m_unclock[i]) {
            unclkindex = i;
            break;
        }
    }
    for(uint8_t i = 0; i < len; i++) {
        if(step_data[i] == m_unclock[(unclkindex + i) % len])
            unclock_cnt++;
    }
    if(clock_cnt == len && unclock_cnt != len) {
        printf("step motor is clockwise\n");
        stepdir = S_CLOCK;
    } else if(clock_cnt != len && unclock_cnt == len) {
        printf("step motor is unclockwise\n");
        stepdir = S_UNCLOCK;
    } else { 
        printf("unknow step direction\n");
        stepdir = S_UNKNOWN;
    }

    return stepdir;
}

static void IRAM_ATTR gpio_isr_handler(void* arg)
{
    uint8_t stepmotor_A1, stepmotor_B2, stepmotor_B1, stepmotor_A2;
    uint32_t gpio_num = 0x00;
    uint32_t que_data = 0x00;

    gpio_num = (uint32_t)arg;

    if(gpio_num == GPIO_INPUT_IO_0 || gpio_num == GPIO_INPUT_IO_1 \
        || gpio_num == GPIO_INPUT_IO_2 || gpio_num == GPIO_INPUT_IO_3) {
        stepmotor_A1 = gpio_get_level(GPIO_INPUT_IO_0); 
        stepmotor_B2 = gpio_get_level(GPIO_INPUT_IO_1); 
        stepmotor_B1 = gpio_get_level(GPIO_INPUT_IO_2); 
        stepmotor_A2 = gpio_get_level(GPIO_INPUT_IO_3); 
        que_data |= 0x0100 | (stepmotor_A1 << 3) | (stepmotor_B2 << 2) | (stepmotor_B1 << 1) | (stepmotor_A2 << 0);
    } else if(gpio_num == GPIO_INPUT_IO_4 || gpio_num == GPIO_INPUT_IO_5 \
        || gpio_num == GPIO_INPUT_IO_6 || gpio_num == GPIO_INPUT_IO_7) {
        stepmotor_A1 = gpio_get_level(GPIO_INPUT_IO_4); 
        stepmotor_B2 = gpio_get_level(GPIO_INPUT_IO_5); 
        stepmotor_B1 = gpio_get_level(GPIO_INPUT_IO_6); 
        stepmotor_A2 = gpio_get_level(GPIO_INPUT_IO_7); 
        que_data |= (stepmotor_A1 << 3) | (stepmotor_B2 << 2) | (stepmotor_B1 << 1) | (stepmotor_A2 << 0);
    }
    xQueueSendFromISR(gpio_evt_queue, &que_data, NULL);
}

static void stepmotor_decode_task(void* arg)
{
    uint32_t io_num;
    uint8_t step_cnt = 0;
    uint32_t step_data[4] = {0};
    uint32_t motordata = 0x00;
    Step_Dir step_dir = S_UNKNOWN;
    uint32_t stepmotor_pos = 0x00;

    for (;;) {
        if (xQueueReceive(gpio_evt_queue, &io_num, portMAX_DELAY)) {
            printf("%ld %ld %ld %ld 0x%02lx\n", (io_num & 0x08) >> 3, (io_num & 0x04) >> 2, (io_num & 0x02) >> 1, (io_num & 0x01) >> 0, io_num);
            //use the first data to specify the stepmotor position. If there are two pos in step_data, we will get S_UNKNOWN.
            //stepmotor_pos = io_num & 0x0100;
            stepmotor_pos = gpio_get_level(GPIO_INPUT_IO_3);
            step_data[step_cnt] = io_num & 0xff;
            step_cnt++;
            if(step_cnt >= (sizeof(step_data) / sizeof(step_data[0]))) {
                step_dir = step_motor_direction(step_data, sizeof(step_data) / sizeof(step_data[0]));
                if(stepmotor_pos) {//step motor A
                    if(step_dir == S_CLOCK) {
                        motordata = (0x01 << 24) | (0xff << 16) | (0x01 << 8) | 0xff;
                        xQueueSend(motor_ctl_queue, &motordata, 0);
                    } else if(step_dir == S_UNCLOCK) {
                        motordata = (0x00 << 24) | (0xff << 16) | (0x00 << 8) | 0xff;
                        xQueueSend(motor_ctl_queue, &motordata, 0);
                    }
                } else {//step motor B
                    if(step_dir == S_CLOCK) {
                        motordata = (0x01 << 24) | (0xff << 16) | (0x00 << 8) | 0xff;
                        xQueueSend(motor_ctl_queue, &motordata, 0);
                    } else if(step_dir == S_UNCLOCK) {
                        motordata = (0x00 << 24) | (0xff << 16) | (0x01 << 8) | 0xff;
                        xQueueSend(motor_ctl_queue, &motordata, 0);
                    }
                }
                step_cnt = 0;
                stepmotor_pos = 0x00;
            }
        }
    }
}

esp_err_t stepmotor_init(void)
{
    //zero-initialize the config structure.
    gpio_config_t io_conf = {};
    //interrupt of falling edge
    io_conf.intr_type = GPIO_INTR_NEGEDGE;
    //bit mask of the pins, use GPIO4/5 here
    io_conf.pin_bit_mask = GPIO_INPUT_PIN_SEL;
    //set as input mode
    io_conf.mode = GPIO_MODE_INPUT;
    //disable pull-up mode
    io_conf.pull_up_en = 1;
    //enable pull-down mode
    io_conf.pull_down_en = 0;
    gpio_config(&io_conf);

    //create a queue to handle gpio event from isr
    gpio_evt_queue = xQueueCreate(10, sizeof(uint32_t));

    xTaskCreate(stepmotor_decode_task, "stepmotor_decode_task", 4096, NULL, 10, NULL);

    //install gpio isr service
    gpio_install_isr_service(ESP_INTR_FLAG_DEFAULT);
    //remove isr handler for gpio number.
    gpio_isr_handler_remove(GPIO_INPUT_IO_0);
    //hook isr handler for step motor A
    //gpio_isr_handler_add(GPIO_INPUT_IO_0, gpio_isr_handler, (void*) GPIO_INPUT_IO_0);
    //gpio_isr_handler_add(GPIO_INPUT_IO_1, gpio_isr_handler, (void*) GPIO_INPUT_IO_1);
    //gpio_isr_handler_add(GPIO_INPUT_IO_2, gpio_isr_handler, (void*) GPIO_INPUT_IO_2);
    //gpio_isr_handler_add(GPIO_INPUT_IO_3, gpio_isr_handler, (void*) GPIO_INPUT_IO_3);

    //hook isr handler for step motor B 
    gpio_isr_handler_add(GPIO_INPUT_IO_4, gpio_isr_handler, (void*) GPIO_INPUT_IO_4);
    gpio_isr_handler_add(GPIO_INPUT_IO_5, gpio_isr_handler, (void*) GPIO_INPUT_IO_5);
    gpio_isr_handler_add(GPIO_INPUT_IO_6, gpio_isr_handler, (void*) GPIO_INPUT_IO_6);
    gpio_isr_handler_add(GPIO_INPUT_IO_7, gpio_isr_handler, (void*) GPIO_INPUT_IO_7);

    return ESP_OK;
}
